clear
% the order of breakpoints listed below is the order used in table lookup
% data.
MS=.01;% Max sample interval
min_st1_inc=.001;% minimum ST1 incremetn that can be used during learning phase
global RG
MAX_PULSES=0;% max number of allowed pulses.  Only allow one pulse per iteration until LUTS are populated, otherwise we'll get noise out of the LUTS
RG=1000;% rounding gain
IFA=round(.01*RG)/RG; % initial foot angle
IHA=round(0*RG)/RG;% initial hip angle
APHorder=2;% initial aph data is 0
T=round(.1*RG)/RG;% initalize T to about .1 seconds.  Reduce Pulse width until final APH=AF*100 for input
ST1=round(0*RG)/RG;
sim_time=2;% length of time to run simulation
do_adj=0;% adjust parameters of an acceleration pulse until desired behavior is achieved
max_pulses=0;
%Note: initial values of breakpoint vectors should be greater than the absolute limits
%of the system to prevent out-of-bound values
%LUT dim. 1
APFBP=10000*[IFA-1/RG IFA IFA+1/RG];% angular position of foot breakpoint vector
%LUT dim. 2
AVFBP=10000*[-1/RG 0 1/RG];% velocity in radians per second.
%LUT dim. 3
AAFBP=[-1/RG 0 1/RG];
%LUT dim 4
APHBP=[IHA-1/RG IHA IHA+1/RG];% angular position of hip breakpoint vector
TLDT=zeros(3,3,3,3); % Table lookup data for acceleration pulse 1/2period.  Note that there are as many dimensions as breakpoint vectors, each dimension has a size = corresponding breakpoint vector length
TLDAF=zeros(3,3,3,3);
TLDST1=zeros(3,3,3,3);
%Find initial acceleration due to initia position and velocity.  LIke
%"hefting" to guage object parameters
APFBP=floor(APFBP*RG)/RG;
AVFBP=floor(AVFBP*RG)/RG;
AAFBP=floor(AAFBP*RG)/RG;
APHBP=floor(APHBP*RG)/RG;
[t,x,apf,avf,aaf,aph,apf_pe,avf_pe,aaf_pe,aph_pe,clk_pe,pulse_cnt,aah]=sim('ipend3',.5);
DATA=[apf_pe(end),avf_pe(end),aaf_pe(end),aph_pe(end),clk_pe(end)];
AF=.5*1/apf_pe(end);% initial high energy "kick".  determined experimentalyy to cause convergence
%LUT dim. 3
APForder=2;% initial ap data is IFA
AVForder=2;% initial av is 0
%Run simulation once to find acceleration due to initial conditions
if aaf(1)~=0 % break point vector must be monotonic, so don't duplicate 0 value
%     [AAFBP AAForder]=sort([init_aaf_val AAFBP]);% first data point in the acceleration break point array.
%     AAForder=find(AAFBP==init_aaf_val);% find where new value is inserted into breakpoint array
    [TLDT,TLDAF,TLDST1,AAFBP,AAForder]=InsertBlankRow(TLDT,TLDAF,TLDST1,AAFBP,aaf_pe(end),3);
    
else
    AAForder=2;% initial aa data is 0
end

%% BEGIN RIGHTING BEHAVIOR
%emit initial guess.  parameters determined experimentally to cause foot
%angle to cross vertical with enogh velocity to continue a monotonic increase
%in foot angle, velocity should be relatively high, APH should be around .4
%radians.

% there is a desired shape to aph.  Each succcessive pulse should reduce
% |aph| by max_aph/4 until max aph<=value that causes top of pendulum to
% move <=1cm.  (.05, for instance)

% T should be adjusted so that max aph=AF/100
%%
TLDT(APForder,AVForder,AAForder,APHorder)=T;
TLDAF(APForder,AVForder,AAForder,APHorder)=41;%AF;
%@42, the 2nd pulse ( intended to take the APH to -.01) doesn't have enough
%energy to cause the first AAF minima to be low enough for the 3rd pusle (
%fixed time/amp) to change the sign.
%@41 and below, necesary conditions are met, but choose 41 t minimize the
%time hip spends at maximum angle
% The first AF has to be determined experimentaly
TLDST1(APForder,AVForder,AAForder,APHorder)=ST1;
MAX_PULSES=MAX_PULSES+1;% allow one additional pulse for next trial
[t,x,apf,avf,aaf,aph,apf_pe,avf_pe,aaf_pe,aph_pe,clk_pe,pulse_cnt,aah]=sim('ipend3',sim_time);
max_pulses=1;
max_pulses_idx=find(diff(clk_pe))+1;% signal vector indices of acceleration pulse endings
max_pulses_idx=max_pulses_idx(max_pulses);
%%
prev_pulse_aaf_sign=0;% direction of foot angular acceleration due to the previous pulse. -1=clockwise
last_AF=AF;% remember the last applied force 
target_aph=-.01;% adjust AF so APH reaches this magnitude
st1_vals=[0.108 0.5];
target_pos=[];% ytarget angular foot position when angular foot velocity =0

AAF_DATA=[]; % AAF at AAV zero crossing
while 1
    %change parameters of motion
    cur_pulse_aaf_sign=0;
    good_sign=-1;
    good_min_val=-1;% test of value to minimize is changing in the desired direction
    prev_good_min_val=-1;% valid values are 0/1, -1 indicates unintialized
    prev_good_sign=-1;
    loop_counter=0;% count time through loop  
    goal_reached=0;
    param_arr=[100*(target_aph-aph_pe(max_pulses_idx)),.1,0];% AF,T,ST1 values    
    param_arr_idx_vec=[1 3 -1];% sequence of array indices to alter.  Terminate process when -1 is reached
    param_arr_idx_vec_idx=1;
    param_arr_idx=param_arr_idx_vec(param_arr_idx_vec_idx);% which parameter are we currently adjusting?=
    par_adj_level=[(10*250)/RG,.25];% parameter adjustment level, begin adjusting applied force at same magnitude as previous pulse, adjust time at minimum possible
    par_adj_dir=[-1*sign(last_AF),1];% parameter adjustment direction can take values 1(positive) -1 ( nehative)
    cur_pulse_count=pulse_cnt(end);% be sure only one pusle is generated, for now.Multiple pulses means system is trying to interpolate values in the LUTs, which won't be valid until training is complete
    prev_pulse_cnt=cur_pulse_count;% there is one pulse at time=0
    cur_min_val=100000;%initialize to some value that can never be attained by the system
    prev_min_val=100000;
    prev_value=[];% history of parameter values used during search   
%% ADD NEW INITAL VALUES TO BREAKPOINT ARRAYS
    [TLDT,TLDAF,TLDST1,APFBP,AVFBP,AAFBP,APHBP,APForder,AVForder,AAForder,APHorder]=InsertBlankRows(TLDT,TLDAF,TLDST1,APFBP,AVFBP,AAFBP,APHBP,apf_pe(max_pulses_idx),avf_pe(max_pulses_idx),aaf_pe(max_pulses_idx),aph_pe(max_pulses_idx));
    DATA=[DATA;apf_pe(max_pulses_idx),avf_pe(max_pulses_idx),aaf_pe(max_pulses_idx),aph_pe(max_pulses_idx),clk_pe(max_pulses_idx)];
%%
%st1_vals=[0.108 0.5 1.434 1.438 1.287 1.342 1.485 1.183 1.241 1.315 1.908
%1.315 1.32 1.536 0 0 0 0];
%1st pulse: magnitude fixed to get desired APH, time fixed to T, ST1
%determinedby first aaf minima after pulse end.  Adjust ST1 so minima is as
%close to 0 as possible witout crossing.  
%2nd pulse: choose ST1 to maximize hang time
%3rd pulse: ST1 set so abs(start(aaf))~abs(end(aaf)).  
%final aaf must be same sign as pulse start aaf.  If this criteria can't
%be met, choose 3rd pulse st1 to maximize hang time and repeat serch.  Once
%a valid pulse is found, store the value of start and end aaf, set mean as
%the threshold for futur pulse start times
% 4th pulse and on: use aaf criteria and threshold to determine times
%sign of ST1 = sign of max(abs(aaf[start,end]))).  if sign changes, adjust
%increment amount and adjust in other direction until minimum resolution
%threshold is reached.
    TLDAF(APForder,AVForder,AAForder,APHorder)=param_arr(1)+0;%amp(max_pulses);%0;0;0;-11.01;
    TLDT(APForder,AVForder,AAForder,APHorder)=T;% f0or now, T is constant     
    if max_pulses>2
        next_ready=0;
        inc=1;
        diff_aaf=[];% track differences in aaf at beginning and end of pulse
        sign_diff_aaf=0;% determine if aaf sign is different at start and end of pulse
        sign_diff_aaf_tog=0;% sign diff toggle

        st1_delta_dir=1;% direction to change ST1. 1=pos, -1=neg
        %st1_delta_inc=1;% time to change ST1 (seconds)
        %st1_vals(max_pulses)=.01;% ST1 initial value
        rep=0;  
        prev_check2=-1;
        if max_pulses>=4
            acc_to_match=END(3);% acceleration at end of last pulse
            start_idx=last_pulse_end_idx;
            end_idx=length(t);
            signal_of_interest=aaf(start_idx:end_idx);
            v_level=crossing(signal_of_interest-acc_to_match);% signal indexes matching given level
            st1_delta_inc=.001;
            st1_vals(max_pulses)=t(v_level(2)+start_idx)-t(start_idx)-st1_delta_inc;% set the initial pulse time
        else
            st1_vals(max_pulses)=.01;
            st1_delta_inc=1;
         end% if max_pulses>=4
         MAX_PULSES=MAX_PULSES+1;
        while ~next_ready
            rep=rep+1;
            disp('pulse# trial inc dir')
            [max_pulses rep st1_delta_inc st1_delta_dir]
            st1_vals(max_pulses)=st1_vals(max_pulses)+(st1_delta_dir*st1_delta_inc);
            %sim_time=sum(st1_vals)+4 % 3 seconds past last pulse+time of current pulse
            TLDST1(APForder,AVForder,AAForder,APHorder)=st1_vals(max_pulses);%ss(max_pulses);%param_arr(3)+0;1.356%1.403%1.072;2.707;0
            sim_time=t(last_pulse_end_idx)+st1_vals(max_pulses)+3;
            [t,x,apf,avf,aaf,aph,apf_pe,avf_pe,aaf_pe,aph_pe,clk_pe,pulse_cnt,aah]=sim('ipend3',sim_time); 
            [min_val last_pulse_end_idx]=min(abs(t-(2*T+clk_pe(max_pulses_idx)+TLDST1(APForder,AVForder,AAForder,APHorder))));  % index into time vector of stop point of lat acc pulse     
            [min_val last_pulse_start_idx]=min(abs(t-(clk_pe(max_pulses_idx)+TLDST1(APForder,AVForder,AAForder,APHorder))));       
            START=[apf(last_pulse_start_idx-1) avf(last_pulse_start_idx-1) aaf(last_pulse_start_idx-1)]        
            END=[apf(last_pulse_end_idx+1) avf(last_pulse_end_idx+1) aaf(last_pulse_end_idx+1)]
            disp('-------')  
            zc_idx=crossing(avf);% zero crossing indexes of AVF
            zc_idx=zc_idx(find(zc_idx>last_pulse_end_idx+1,1));% first zero crossing of AVF after end of pulse
            if abs(aaf(zc_idx))>=abs(aaf(zc_idx+1))% if the angular foot acceleration to the left of velocity zero crossing is >= aaf to the right
                aaf_zc=aaf(zc_idx);
            else
                aaf_zc=aaf(zc_idx+1);
            end            
            check1=0;
            check1=st1_delta_inc==min_st1_inc;
           
            % we add a value to velocity equal in sign and magnitude to
            % final pulse velocity.  This should result in a new zero
            % crossing a little ahead of the initial velocity zero crossing
            if ~isempty(zc_idx)%&&~isempty(vc_idx)% check that velocity crosses zero and th
                check2=1;
            else
                check2=0;
            end
            if (((check1==1) && (check2 ==1)) && (prev_check2==0)) || ((check2 ==1) && (abs(aaf_zc)<5))
                next_ready=1;
                if size(AAF_DATA,1)>=1
                    AAF_DATA(size(AAF_DATA,1),3)=t(last_pulse_start_idx);
                end
                AAF_DATA=[AAF_DATA;aaf_zc t(zc_idx) 0];
            else
                prev_dir=st1_delta_dir;
                if sign(START(2))==(sign(END(2)))% then start and end aaf are on opposite sides of 0 (desired outcome)
                    if sign(START(2))==(-1*sign(TLDAF(APForder,AVForder,AAForder,APHorder)))%isempty(zc_idx)% aaf at start of pulse has to be a bit higher then the acceleration at the end of pulse
                        st1_delta_dir=1;
                    else
                        if isempty(zc_idx)
                            st1_delta_dir=-1;
                        else
                            st1_delta_dir=1;% go forward
                        end
                    end
                else% start and end aaf are the same sign, pulse occurs in a down/upward sloping region
                    st1_delta_dir=1;% go back
                end   
                if prev_dir==(-1*st1_delta_dir)% reduce incrment
                    st1_delta_inc=max([min_st1_inc st1_delta_inc/2])
                end
            end% if (((check1==1) && (check2 ==1)) && (prev_check2==0))...
            prev_check2=check2;% remember the previous value
        end% while ~next_ready
        if (max_pulses>2) && isempty(target_pos)% need to find position target        
            velocity_signal=avf(last_pulse_end_idx+1:end);% angular foot velocity signal starting from the end of the last acceleration pulse, going to end of signal
            position_signal=aaf(last_pulse_end_idx+1:end);
            [pre_zc_idx,t0]=crossing(velocity_signal,1:length(velocity_signal));% zero crossing index of velocity and position signals
            delta_t=t0-pre_zc_idx;
            delta_pos=position_signal(pre_zc_idx)-position_signal(pre_zc_idx);
            target_pos=position_signal(pre_zc_idx)+delta_t*delta_pos;
        end% if (max_pulses>3) && isempty(target_pos)         
    else
        TLDST1(APForder,AVForder,AAForder,APHorder)=st1_vals(max_pulses);%initial 2 ST1 values determined experimentally
        MAX_PULSES=MAX_PULSES+1;
        [t,x,apf,avf,aaf,aph,apf_pe,avf_pe,aaf_pe,aph_pe,clk_pe,pulse_cnt,aah]=sim('ipend3',sim_time); 
        [min_val last_pulse_end_idx]=min(abs(t-(2*T+clk_pe(max_pulses_idx)+TLDST1(APForder,AVForder,AAForder,APHorder))));       
        [min_val last_pulse_start_idx]=min(abs(t-(clk_pe(max_pulses_idx)+TLDST1(APForder,AVForder,AAForder,APHorder))));       
        START=[apf(last_pulse_start_idx-1) avf(last_pulse_start_idx-1) aaf(last_pulse_start_idx-1)]        
        END=[apf(last_pulse_end_idx+1) avf(last_pulse_end_idx+1) aaf(last_pulse_end_idx+1)]
    end% if max_pulses>2
    disp('-------') 

%%
    max_pulses=max_pulses+1;% the number of pulses actively generated.  The maximum number of pulses to extract from signal.  Anything past this is generated from lookup tables
    max_pulses_idx=find(diff(clk_pe))+1;% signal vector indices of acceleration pulse endings
    max_pulses_idx=max_pulses_idx(max_pulses);        
    last_AF=param_arr(1);
    if aph_pe(max_pulses_idx)>0
        target_aph=-0.01;
    else
        target_aph=0.01;
    end
    %sim_time=clk_pe(end)+4;       
end
%83.333 -115  35 -77
%60 -62 5 -5 8


%     do_adj=1;
%     MAX_PULSES=MAX_PULSES+1;% allow one additional pulse for next trial
%     delete MEMORY.mat;
%     save 'MEMORY' *BP TLD* IFA MS RG MAX_PULSES;% save learned data and system state
%
